import pandas as pd
import matplotlib.pyplot as plt
from scipy.signal import lfilter
import numpy as np

# === 1. Load CSV ===
file = "../config/exoskeleton1_raw.csv"   # <-- replace with your CSV filename
df = pd.read_csv(file)

# === 2. Basic info ===
print(df.head())
print(df.describe())

# === Create Subplots ===
fig, axs = plt.subplots(2, 2, figsize=(12, 8), sharex=True)

n = len(df)
start = int(n * (3/7))
end   = int(n * (4/7))
df_mid = df.iloc[start:end]


# --- IIR filter coefficients from H(z) ---
b = [0.0279, 0.0557, 0.0279]  # numerator
a = [1, -1.4755, 0.5869]      # denominator (a[0] must be 1)


# --- Apply filter ---
lr_filtered = lfilter(b, a, df["tof.distance"].to_numpy())


# --- Initialize ---
F_lr = np.zeros_like(lr_filtered)  # filtered LRS
cumsum = 0.0

# --- Real-time baseline removal ---
for i in range(len(lr_filtered)):
    if i == 0:
        F_lr[i] = lr_filtered[i]  # first sample, no previous mean
    else:
        mean_prev = np.mean(lr_filtered[:i])  # mean of all previous samples
        F_lr[i] = lr_filtered[i] - mean_prev

# --- keep middle part
n = len(F_lr)
start = int(n * (3/7))
end   = int(n * (4/7))
F_lr_mid = F_lr[start:end]

# --- Moving average filter coefficients ---
N = 33  # number of taps
b = np.ones(N) / N  # numerator coefficients (average)
a = [1]              # denominator (FIR)

# --- Apply FIR filter ---
F_gy = lfilter(b, a, df_mid["leftImuData"])

# --- Subplot 1: TOF Distance ---
axs[0,0].plot(df_mid["Cnttime"].to_numpy(), df_mid["tof.distance"].to_numpy(), label="Flitered laser-ranging signal", color="purple")
axs[0,0].set_xlabel("Cnttime")
axs[0,0].set_ylabel("Normalized Distance")
axs[0,0].set_title("Flitered laser-ranging signal")
axs[0,0].legend()
axs[0,0].grid(True)


axs[0,1].plot(df_mid["Cnttime"].to_numpy(), F_lr_mid, label="TOF Distance", color="purple")
axs[0,1].set_xlabel("Cnttime")
axs[0,1].set_ylabel("Distance (m)")
axs[0,1].set_title("TOF Distance")
axs[0,1].legend()
axs[0,1].grid(True)




# --- Subplot 2: IMU Data ---
axs[1,0].plot(df_mid["Cnttime"].to_numpy(), df_mid["leftImuData"].to_numpy(), label="Left IMU")
# axs[0].plot(df_mid["Cnttime"].to_numpy(), df_mid["rightImuData"].to_numpy(), label="Right IMU")
axs[1,0].set_ylabel("IMU Data")
axs[1,0].set_title("Left vs Right IMU Data")
axs[1,0].legend()
axs[1,0].grid(True)



# --- Subplot 2: IMU Data ---
axs[1,1].plot(df_mid["Cnttime"].to_numpy(), F_gy, label="Left IMU")
# axs[0].plot(df_mid["Cnttime"].to_numpy(), df_mid["rightImuData"].to_numpy(), label="Right IMU")
axs[1,1].set_ylabel("IMU Data")
axs[1,1].set_title("Left vs Right IMU Data")
axs[1,1].legend()
axs[1,1].grid(True)


# Adjust layout
plt.tight_layout()
plt.show()